package edu.wpi.first.wpilibj.defaultCode;

/**
 *
 * @author buttercream
 */
public class Constants
{
    public static int DRIVE_MTR_TOOTH_COUNT = 42; //update
    public static float WHEEL_CIRC = 12; //in inches, update
    public static int DRIVE_MTR_SAMPLE_RATE = 20; //how often to sample the
                                                  //drive motor rate

    //Teeth measurements for the lifter
    public static int LIFTER_NEUTRAL_TEETH = 0; //neutral state in teeth
    public static int LIFTER_HEIGHT_ONE_TEETH = 10; //bottom row height in teeth
    public static int LIFTER_HEIGHT_TWO_TEETH = 20; //middle row height in teeth
    public static int LIFTER_HEIGHT_THREE_TEETH = 30; //top row height in teeth

    //Wrist angles
    public static int WRIST_NEUTRAL_ANGLE = 0; //angle the wrist should be at
                                               //while nothing else is occuring
    public static int WRIST_NEUTRAL_TEETH = 0; //teeth measurement for neutral
                                               //angle state
    public static int WRIST_ANGLE = 45; //desired angle the wrist should be
                                        //when placing the tubes on the peg
    public static int WRIST_TEETH = 45; //teeth mesaurement for placement angle
                                        //state

    //Teeth measurements for the WRIST
    public static int ARM_NEUTRAL_TEETH = 0; //neutral state in teeth
    public static int ARM_HEIGHT_ONE_TEETH = 10; //bottom row height in teeth
    public static int ARM_HEIGHT_TWO_TEETH = 20; //middle row height in teeth
    public static int ARM_HEIGHT_THREE_TEETH = 30; //top row height in teeth

    public static double PI = 3.14159265358979323846;

    public static int SNAP_0_BUTTON_NUM = 4;

    public static int SNAP_90_BUTTON_NUM = 5;

    public static int SNAP_180_BUTTON_NUM = 6;

    public static int SNAP_270_BUTTON_NUM = 7;

    public static int STEER_ENC_NUM_TICKS = 1024;

    public static int DRIVE_ENC_NUM_TICKS = 250;

    
    public static int FL_RR_STEER_MTR = 6;

    // temporarily switch from 3 to 5 ---
    public static int FR_RL_STEER_MTR = 5;  // 3

    public static int FL_DRIVE_ENC = 1;

    public static int FL_DRIVE_MOTOR = 1;

    public static int RL_DRIVE_ENC = 2;

    public static int RL_DRIVE_MOTOR = 2;

    public static int FR_DRIVE_ENC = 4;

    public static int FR_DRIVE_MOTOR = 4;

    public static int RR_DRIVE_ENC = 5;

    public static int RR_DRIVE_MOTOR = 5;

    public static int COMPRESSOR = 7;

    public static double P_DRIVE_CONST = .5;//.5

    public static double I_DRIVE_CONST = 0;//0.11

    public static double D_DRIVE_CONST = 0;

    public static double P_STEER_CONST = .03;

    public static double I_STEER_CONST = 0;

    public static double D_STEER_CONST = 0;

    public static double PID_TOLERANCE = .5;

    public static double STEER_PID_TOLERANCE = .3;  // according to javadocs -- 15 = 15%

    public static int MAINTENANCE_MODE_BUTTON_NUM = 8;

    public static double PID_CLOCK_CYCLE = .005;

    public static double DISTANCE_PER_PULSE = 13.5/1440; //confirm

    public static double MAX_RATE = 2000; //wag

    public static int MODULE_NUM = 4;

    public static int CRIO_DIGITAL_SIDECAR = 4;
    public static int CRIO_ANALOG_BREAKOUT = 1;
    public static int CRIO_PNEUMATICS = 7;
    public static int CLAW_SOLENOID = 1;

    public static boolean USE_THREE_JOYSTICKS = true;
    // see Controller.java for reassignment

    // Joystick assignments
    public static int JOYSTICK_DRIVE = 1;
    public static int JOYSTICK_ROTATION = 2;
    public static int JOYSTICK_ARM = 3;

    // BUTTON Assignments -- Joystick 3 -- Arm
    public static int BTN_CLAW_GRAB = 1;
    public static int BTN_CLAW_RELEASE_WITH_GRAB = 2;
    public static int BTN_FLOOR = 3;
    public static int BTN_WRIST_MANUAL_DOWN = 4;
    public static int BTN_TUCKED = 5;
    public static int BTN_WRIST_MANUAL_UP = 6;
    public static int BTN_TOP_ROW_CENTER = 7;
    public static int BTN_TOP_ROW_SIDE = 8;
    public static int BTN_MIDDLE_ROW_CENTER = 9;
    public static int BTN_MIDDLE_ROW_SIDE = 10;
    public static int BTN_BOTTOM_ROW_CENTER = 11;
    public static int BTN_BOTTOM_ROW_SIDE = 12;

 
    // BUTTON Assignments -- Joystick 2 -- MiniBot and Twist
    public static int BTN_MINIBOT_DEPLOY_ONE = 6;
    public static int BTN_MINIBOT_DEPLOY_TWO = 11;
    public static int BTN_MINITBOT_RETRACT = 5;

    public static int LIFTER_TOP_LIMIT_SWITCH = 1;
    public static int LIFTER_BOTTOM_LIMIT_SWITCH = 2;
    public static int LIFTER_MOTOR = 7;
    public static double LIFTER_RATE = 1.0;   // speed of lifter motor

    // Arm positions
    public static double ARM_NEUTRAL = 7.0;   // TODO
    public static double ARM_FLOOR = 7.0;   // TODO
    public static double ARM_TUCKED = 7.0;   // TODO
    public static double ARM_TOP_ROW_SIDE = 7.0;   // TODO
    public static double ARM_TOP_ROW_CENTER = 7.0;   // TODO
    public static double ARM_MIDDLE_ROW_SIDE = 7.0;   // TODO
    public static double ARM_MIDDLE_ROW_CENTER = 7.0;   // TODO
    public static double ARM_BOTTOM_ROW_SIDE = 7.0;   // TODO
    public static double ARM_BOTTOM_ROW_CENTER = 7.0;   // TODO

    public static double ARM_PID_P = 7.0;   // TODO
    public static double ARM_PID_I = 0.0;   // TODO
    public static double ARM_PID_D = 0.0;   // TODO 

    // Wrist Positions

    public static double WRIST_NEUTRAL = 0.0;   // TODO
    public static double WRIST_FLOOR = 7.0;   // TODO
    public static double WRIST_TUCKED = 7.0;   // TODO
    public static double WRIST_TOP_ROW = 7.0;   // TODO
    public static double WRIST_MIDDLE_ROW= 7.0;   // TODO
    public static double WRIST_BOTTOM_ROW= 7.0;   // TODO

    public static double WRIST_PID_P = 7.0;   // TODO
    public static double WRIST_PID_I = 0.0;   // TODO
    public static double WRIST_PID_D = 0.0;   // TODO

    // Wrist / Arm Channel and Motor
    public static int WRIST_CHNL = 4;
    public static int ARM_CHNL = 3;

    public static int ARM_MOTOR = 8;
    public static int WRIST_MOTOR = 9;

    // Compressor
    public static int COMPRESOR_PRESSURE_CHNL = 13;
    public static int COMPRESOR_RELAY_CHNL = 10;

    // Mini Bot
    public static int SOL_BOT_DEPLOY = 3;
    public static int SOL_DRAWER_DEPLOY = 2;
    public static int MINIBOT_LIMIT_SWITCH = 1;

    // for Limit switches -- voltage -- 0 = on 5 = off
    static double VOLTAGE_THRESHHOLD = 2.5;

}
